/*
 *   MotionManager.cpp
 *
 *   Author: ROBOTIS
 *
 */

#include <stdio.h>
#include <math.h>
#include "../include/FSR.h"
#include "../include/MX28.h"
#include "../include/MotionManager.h"
#include "../include/MotorController.h"
#include "../include/ATMega328p.h"

using namespace Robot;

MotionManager* MotionManager::m_UniqueInstance = new MotionManager();

MotionManager::MotionManager() :
        m_CM730(0),
        m_ProcessEnable(true), //TODO false by default
        m_Enabled(false),
        m_IsRunning(false),
        m_IsThreadRunning(false),
        m_IsLogging(false),
        DEBUG_PRINT(false)//TODO true by default
{
    for(int i = 0; i < JointData::NUMBER_OF_JOINTS; i++)
        m_Offset[i] = 0;
}

MotionManager::~MotionManager()
{
}

bool MotionManager::Initialize()//CM730 *cm730)
{
	int value, error;

	//TODO m_CM730 = cm730;
	m_Enabled = false;
	m_ProcessEnable = true;

//	if(m_CM730->Connect() == false)
//	{
//		if(DEBUG_PRINT == true)
//			fprintf(stderr, "Fail to connect CM-730\n");
//		return false;
//	}

	for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++)
	{
		if(DEBUG_PRINT == true)
			fprintf(stderr, "ID:%d initializing...", id);
		
//TODO	Read present for initial posisiotns

//		if(m_CM730->ReadWord(id, MX28::P_PRESENT_POSITION_L, &value, &error) == CM730::SUCCESS)
//		{
			value= MotorController::GetInstance()->readMotorPosition(id);
			MotionStatus::m_CurrentJoints.SetValue(id, value);
			MotionStatus::m_CurrentJoints.SetEnable(id, true);

			if(DEBUG_PRINT == true){
				fprintf(stderr, "[%d] Success\n", value);
			}
//		}
//		else
//		{
//			MotionStatus::m_CurrentJoints.SetEnable(id, false);
//
//			if(DEBUG_PRINT == true)
//				fprintf(stderr, " Fail\n");
//		}
	}

	//TODO clib status is 0 by default. making 1 will alow robot to go straight into walking?
	m_CalibrationStatus = 0;
	m_FBGyroCenter = 512;
	m_RLGyroCenter = 512;

	return true;
}

bool MotionManager::Reinitialize()
{
	m_ProcessEnable = false;

	m_CM730->DXLPowerOn();

	int value, error;
	for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++)
	{
		if(DEBUG_PRINT == true)
			fprintf(stderr, "ID:%d initializing...", id);
		
		if(m_CM730->ReadWord(id, MX28::P_PRESENT_POSITION_L, &value, &error) == CM730::SUCCESS)
		{
			MotionStatus::m_CurrentJoints.SetValue(id, value);
			MotionStatus::m_CurrentJoints.SetEnable(id, true);

			if(DEBUG_PRINT == true)
				fprintf(stderr, "[%d] Success\n", value);
		}
		else
		{
			MotionStatus::m_CurrentJoints.SetEnable(id, false);

			if(DEBUG_PRINT == true)
				fprintf(stderr, " Fail\n");
		}
	}

	m_ProcessEnable = true;
	return true;
}

void MotionManager::StartLogging()
{
    char szFile[32] = {0,};

    int count = 0;
    while(1)
    {
        sprintf(szFile, "Logs/Log%d.csv", count);
        if(0 != access(szFile, F_OK))
            break;
        count++;
		if(count > 256) return;
    }

    m_LogFileStream.open(szFile, std::ios::out);
    for(int id = 1; id < JointData::NUMBER_OF_JOINTS; id++)
        m_LogFileStream << "ID_" << id << "_GP,ID_" << id << "_PP,";
    m_LogFileStream << "GyroFB,GyroRL,AccelFB,AccelRL,L_FSR_X,L_FSR_Y,R_FSR_X,R_FSR_Y" << std::endl;

    m_IsLogging = true;
}

void MotionManager::StopLogging()
{
    m_IsLogging = false;
    m_LogFileStream.close();
}

//TODO
void MotionManager::LoadINISettings(minIni* ini)
{
    LoadINISettings(ini, OFFSET_SECTION);
   // printf("LoadINISettings: MotionManager\n");
}
void MotionManager::LoadINISettings(minIni* ini, const std::string &section)
{
    int ivalue = INVALID_VALUE;

    for(int i = 1; i < JointData::NUMBER_OF_JOINTS; i++)
    {
        char key[10];
        sprintf(key, "ID_%.2d", i);
        if((ivalue = ini->geti(section, key, INVALID_VALUE)) != INVALID_VALUE)  m_Offset[i] = ivalue;
    }
}
void MotionManager::SaveINISettings(minIni* ini)
{
    SaveINISettings(ini, OFFSET_SECTION);
}
void MotionManager::SaveINISettings(minIni* ini, const std::string &section)
{
    for(int i = 1; i < JointData::NUMBER_OF_JOINTS; i++)
    {
        char key[10];
        sprintf(key, "ID_%.2d", i);
        ini->put(section, key, m_Offset[i]);
    }
}

#define GYRO_WINDOW_SIZE    100
#define ACCEL_WINDOW_SIZE   30
#define MARGIN_OF_SD        2.0


void MotionManager::Process()
{
   // printf("Starting MM Process()\n");
    m_CalibrationStatus = 1;

    if(m_ProcessEnable == false || m_IsRunning == true){
    //    printf("Process was not enabled\n");

        return;
    }

    m_IsRunning = true;

    // calibrate gyro sensor
//    if(m_CalibrationStatus == 0 || m_CalibrationStatus == -1)
//    {
//        printf("Getting gyro data to calibrate\n");
//        static int fb_gyro_array[GYRO_WINDOW_SIZE] = {512,};
//        static int rl_gyro_array[GYRO_WINDOW_SIZE] = {512,};
//        static int buf_idx = 0;
//
//        if(buf_idx < GYRO_WINDOW_SIZE)
//        {
//            if(m_CM730->m_BulkReadData[CM730::ID_CM].error == 0)
//            {
//                fb_gyro_array[buf_idx] = m_CM730->m_BulkReadData[CM730::ID_CM].ReadWord(CM730::P_GYRO_Y_L);
//                rl_gyro_array[buf_idx] = m_CM730->m_BulkReadData[CM730::ID_CM].ReadWord(CM730::P_GYRO_X_L);
//                buf_idx++;
//            }
//        }
//        else
//        {
//            double fb_sum = 0.0, rl_sum = 0.0;
//            double fb_sd = 0.0, rl_sd = 0.0;
//            double fb_diff, rl_diff;
//            double fb_mean = 0.0, rl_mean = 0.0;
//
//            buf_idx = 0;
//
//            for(int i = 0; i < GYRO_WINDOW_SIZE; i++)
//            {
//                fb_sum += fb_gyro_array[i];
//                rl_sum += rl_gyro_array[i];
//            }
//            fb_mean = fb_sum / GYRO_WINDOW_SIZE;
//            rl_mean = rl_sum / GYRO_WINDOW_SIZE;
//
//            fb_sum = 0.0; rl_sum = 0.0;
//            for(int i = 0; i < GYRO_WINDOW_SIZE; i++)
//            {
//                fb_diff = fb_gyro_array[i] - fb_mean;
//                rl_diff = rl_gyro_array[i] - rl_mean;
//                fb_sum += fb_diff * fb_diff;
//                rl_sum += rl_diff * rl_diff;
//            }
//            fb_sd = sqrt(fb_sum / GYRO_WINDOW_SIZE);
//            rl_sd = sqrt(rl_sum / GYRO_WINDOW_SIZE);
//
//            if(fb_sd < MARGIN_OF_SD && rl_sd < MARGIN_OF_SD)
//            {
//                m_FBGyroCenter = (int)fb_mean;
//                m_RLGyroCenter = (int)rl_mean;
//                m_CalibrationStatus = 1;
//                if(DEBUG_PRINT == true)
//                    fprintf(stderr, "FBGyroCenter:%d , RLGyroCenter:%d \n", m_FBGyroCenter, m_RLGyroCenter);
//            }
//            else
//            {
//                m_FBGyroCenter = 512;
//                m_RLGyroCenter = 512;
//                m_CalibrationStatus = -1;
//            }
//        }
//    }

    if(m_CalibrationStatus == 1 && m_Enabled == true)
    {
//TODO uses cm730
//    	printf("Getting gyro data for inertial measurements\n");
        static int fb_array[ACCEL_WINDOW_SIZE] = {512,};
        static int buf_idx = 0;
//        if(m_CM730->m_BulkReadData[CM730::ID_CM].error == 0)
//        {
//            MotionStatus::FB_GYRO = m_CM730->m_BulkReadData[CM730::ID_CM].ReadWord(CM730::P_GYRO_Y_L) - m_FBGyroCenter;
//            MotionStatus::RL_GYRO = m_CM730->m_BulkReadData[CM730::ID_CM].ReadWord(CM730::P_GYRO_X_L) - m_RLGyroCenter;
//            MotionStatus::RL_ACCEL = m_CM730->m_BulkReadData[CM730::ID_CM].ReadWord(CM730::P_ACCEL_X_L);
//            MotionStatus::FB_ACCEL = m_CM730->m_BulkReadData[CM730::ID_CM].ReadWord(CM730::P_ACCEL_Y_L);
//            fb_array[buf_idx] = MotionStatus::FB_ACCEL;
//            if(++buf_idx >= ACCEL_WINDOW_SIZE) buf_idx = 0;
//        }
//        printf("tried to read inertial data\n");
//        int sum = 0, avr = 512;
//        for(int idx = 0; idx < ACCEL_WINDOW_SIZE; idx++){
//            sum += fb_array[idx];
//        }
//        avr = sum / ACCEL_WINDOW_SIZE;
//
//        if(avr < MotionStatus::FALLEN_F_LIMIT){
//            MotionStatus::FALLEN = FORWARD;
//        }
//        else if(avr > MotionStatus::FALLEN_B_LIMIT){
//            MotionStatus::FALLEN = BACKWARD;
//        }
//        else{
//            MotionStatus::FALLEN = STANDUP;
//        }
//TODO
//        ATMega328p::GetInstance()->UpdateInertialMeasurements();
//        MotionStatus::FB_GYRO= ATMega328p::GetInstance()->getFB_GYRO();
//        MotionStatus::FB_GYRO= ATMega328p::GetInstance()->getRL_GYRO();
//        MotionStatus::FB_GYRO= ATMega328p::GetInstance()->getFB_ACCEL();
//        MotionStatus::FB_GYRO= ATMega328p::GetInstance()->getRL_ACCEL();

        if(m_Modules.size() != 0)
        {
        	if(DEBUG_PRINT == true)
        		printf("Modules are present\n");

            for(std::list<MotionModule*>::iterator i = m_Modules.begin(); i != m_Modules.end(); i++)
            {
                (*i)->Process();
                for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++)
                {
                    if((*i)->m_Joint.GetEnable(id) == true)
                    {
                        MotionStatus::m_CurrentJoints.SetSlope(id, (*i)->m_Joint.GetCWSlope(id), (*i)->m_Joint.GetCCWSlope(id));
                        MotionStatus::m_CurrentJoints.SetValue(id, (*i)->m_Joint.GetValue(id));
                     //   printf("setting angle for %d\n", id);

                        MotionStatus::m_CurrentJoints.SetPGain(id, (*i)->m_Joint.GetPGain(id));
                        MotionStatus::m_CurrentJoints.SetIGain(id, (*i)->m_Joint.GetIGain(id));
                        MotionStatus::m_CurrentJoints.SetDGain(id, (*i)->m_Joint.GetDGain(id));
                    }
                }
            }
        }

        int param[JointData::NUMBER_OF_JOINTS * MX28::PARAM_BYTES];
        int n = 0;
        int joint_num = 0;
        if(DEBUG_PRINT == true)
        	printf("Goal Positions!!!! \n");
        //        for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++)
        //        {
        //            if(MotionStatus::m_CurrentJoints.GetEnable(id) == true)
        //            {
        //                param[n++] = id;
        //#ifdef MX28_1024
        //                param[n++] = MotionStatus::m_CurrentJoints.GetCWSlope(id);
        //                param[n++] = MotionStatus::m_CurrentJoints.GetCCWSlope(id);
        //#else
        //                param[n++] = MotionStatus::m_CurrentJoints.GetDGain(id);
        //                param[n++] = MotionStatus::m_CurrentJoints.GetIGain(id);
        //                param[n++] = MotionStatus::m_CurrentJoints.GetPGain(id);
        //
        //                param[n++] = 0;
        //#endif
        //                param[n++] = CM730::GetLowByte(MotionStatus::m_CurrentJoints.GetValue(id) + m_Offset[id]);//converts goal motor value into low byte format
        //                param[n++] = CM730::GetHighByte(MotionStatus::m_CurrentJoints.GetValue(id) + m_Offset[id]);//converts goal motor value into high byte format
        //                joint_num++;
        //            }
        //
        //            if(DEBUG_PRINT == true){
        //
        //                fprintf(stderr, "ID[%d] : %d \n", id, MotionStatus::m_CurrentJoints.GetValue(id));
        //            }
        //        }
        std::vector<int> ids;
        std::vector<int> p_data;
        std::vector<int> i_data;
        std::vector<int> d_data;
        std::vector<int> gp_data;

        ids.resize(JointData::NUMBER_OF_JOINTS);
        p_data.resize(JointData::NUMBER_OF_JOINTS);
        i_data.resize(JointData::NUMBER_OF_JOINTS);
        d_data.resize(JointData::NUMBER_OF_JOINTS);
        gp_data.resize(JointData::NUMBER_OF_JOINTS);

        for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++)
        {
        	if(MotionStatus::m_CurrentJoints.GetEnable(id) == true)
        	{
        		ids[n]= id; //param[n++] = id;
//#ifdef MX28_1024
        		//                param[n++] = MotionStatus::m_CurrentJoints.GetCWSlope(id);
        		//                param[n++] = MotionStatus::m_CurrentJoints.GetCCWSlope(id);
//#else

        		p_data[n]= MotionStatus::m_CurrentJoints.GetPGain(id); //param[n++] = MotionStatus::m_CurrentJoints.GetDGain(id);
        		i_data[n]= MotionStatus::m_CurrentJoints.GetIGain(id); //param[n++] = MotionStatus::m_CurrentJoints.GetIGain(id);
        		d_data[n]= MotionStatus::m_CurrentJoints.GetDGain(id); //param[n++] = MotionStatus::m_CurrentJoints.GetPGain(id);

        		//param[n++] = 0;
//#endif
        		//TODO not part of Darwin coce. This is trying to offset the strange hip pitch angles
        		if(id==3){ //TODO Hip Roll
            		gp_data[n]= MotionStatus::m_CurrentJoints.GetValue(id) - (m_Offset[id] + 600);

        		}
        		else if(id==4){ //TODO Hip Roll
            		gp_data[n]= MotionStatus::m_CurrentJoints.GetValue(id) - (m_Offset[id] - 600);

        		}
        		else if(id==9){ //TODO Hip Roll
            		gp_data[n]= MotionStatus::m_CurrentJoints.GetValue(id) - (m_Offset[id] + 200);

        		}
        		else if(id==10){ //TODO hip roll
            		gp_data[n]= MotionStatus::m_CurrentJoints.GetValue(id) - m_Offset[id];

        		}
        		//11 and 12 are hip pitch
        		else if(id==11){ //TODO temporary fix
            		gp_data[n]= MotionStatus::m_CurrentJoints.GetValue(id) + m_Offset[id] - 100;

        		}
        		else if(id==12){ //TODO temporary fix
            		gp_data[n]= MotionStatus::m_CurrentJoints.GetValue(id) + m_Offset[id] + 100;

        		}
        		//15 and 16 are ankle pitch
        		else if(id==15){ //TODO temporary fix
            		gp_data[n]= MotionStatus::m_CurrentJoints.GetValue(id) + m_Offset[id] - 140;

        		}
        		else if(id==16){ //TODO temporary fix
            		gp_data[n]= MotionStatus::m_CurrentJoints.GetValue(id) - m_Offset[id] + 140;

        		}
        		else if(id==17){ //TODO ankle roll
            		gp_data[n]= MotionStatus::m_CurrentJoints.GetValue(id) - m_Offset[id] - 50;

        		}

        		else if(id==18){ //TODO temporary fix
            		gp_data[n]= MotionStatus::m_CurrentJoints.GetValue(id) - m_Offset[id];

        		}

        		else{
        		gp_data[n]= MotionStatus::m_CurrentJoints.GetValue(id) + m_Offset[id];
        		}
        		n++;
        		joint_num++;
        	}

        	if(DEBUG_PRINT == true){
        		fprintf(stderr, "ID[%d] : %d \n", id, MotionStatus::m_CurrentJoints.GetValue(id));
        	}
        }

        if(joint_num > 0){
        	//#ifdef MX28_1024
        	//           // m_CM730->SyncWrite(MX28::P_CW_COMPLIANCE_SLOPE, MX28::PARAM_BYTES, joint_num, param); //if using different MX28 settings?
        	//#else
        	if(DEBUG_PRINT == true)
        		printf("Ready to move legs!!!\n");

        	MotorController::GetInstance()->sendSyncWrite(ids, MX28::P_P_GAIN, MX28::P_BYTE, p_data);
        	MotorController::GetInstance()->sendSyncWrite(ids, MX28::P_I_GAIN, MX28::P_BYTE, i_data);
        	MotorController::GetInstance()->sendSyncWrite(ids, MX28::P_D_GAIN, MX28::P_BYTE, d_data);
        	MotorController::GetInstance()->sendSyncWrite(ids, MX28::P_GOAL_POSITION_L, MX28::P_WORD, gp_data);


        	//  TODO syncwrite
        	//            m_CM730->SyncWrite(MX28::P_D_GAIN, MX28::PARAM_BYTES, joint_num, param);//this actually writes P,I,D, and Goal Position to motors in the same packet
        	//			MotorController::GetInstance()->SyncWrite(MX28::P_D_GAIN, MX28::PARAM_BYTES, joint_num, param);//this actually writes P,I,D, and Goal Position to motors in the same packet

        	if(DEBUG_PRINT == true)
        		printf("Moving legs\n");
        	//#endif
        }
    }
    if(DEBUG_PRINT == true)
    	printf("ABout to bulk read\n");
    //TODO uses cm730
//    m_CM730->BulkRead();
    if(DEBUG_PRINT == true)
    	printf("Did a bulk read! \n");

//uses cm730
//    if(m_IsLogging)
//    {
//    	printf("LOGGING!\n");
//        for(int id = 1; id < JointData::NUMBER_OF_JOINTS; id++)
//            m_LogFileStream << MotionStatus::m_CurrentJoints.GetValue(id) << "," << m_CM730->m_BulkReadData[id].ReadWord(MX28::P_PRESENT_POSITION_L) << ",";
//
//        m_LogFileStream << m_CM730->m_BulkReadData[CM730::ID_CM].ReadWord(CM730::P_GYRO_Y_L) << ",";
//        m_LogFileStream << m_CM730->m_BulkReadData[CM730::ID_CM].ReadWord(CM730::P_GYRO_X_L) << ",";
//        m_LogFileStream << m_CM730->m_BulkReadData[CM730::ID_CM].ReadWord(CM730::P_ACCEL_Y_L) << ",";
//        m_LogFileStream << m_CM730->m_BulkReadData[CM730::ID_CM].ReadWord(CM730::P_ACCEL_X_L) << ",";
//        m_LogFileStream << m_CM730->m_BulkReadData[FSR::ID_L_FSR].ReadByte(FSR::P_FSR_X) << ",";
//        m_LogFileStream << m_CM730->m_BulkReadData[FSR::ID_L_FSR].ReadByte(FSR::P_FSR_Y) << ",";
//        m_LogFileStream << m_CM730->m_BulkReadData[FSR::ID_R_FSR].ReadByte(FSR::P_FSR_X) << ",";
//        m_LogFileStream << m_CM730->m_BulkReadData[FSR::ID_R_FSR].ReadByte(FSR::P_FSR_Y) << ",";
//        m_LogFileStream << std::endl;
//    }
//
//    if(m_CM730->m_BulkReadData[CM730::ID_CM].error == 0)
//        MotionStatus::BUTTON = m_CM730->m_BulkReadData[CM730::ID_CM].ReadByte(CM730::P_BUTTON);
//
    m_IsRunning = false;
}

void MotionManager::SetEnable(bool enable)
{
	m_Enabled = enable;
	if(m_Enabled == true){
		//TODO
		//m_CM730->WriteWord(CM730::ID_BROADCAST, MX28::P_MOVING_SPEED_L, 0, 0);
	}
}

void MotionManager::AddModule(MotionModule *module)
{
	module->Initialize();
	m_Modules.push_back(module);
}

void MotionManager::RemoveModule(MotionModule *module)
{
	m_Modules.remove(module);
}

void MotionManager::SetJointDisable(int index)
{
    if(m_Modules.size() != 0)
    {
        for(std::list<MotionModule*>::iterator i = m_Modules.begin(); i != m_Modules.end(); i++)
            (*i)->m_Joint.SetEnable(index, false);
    }
}
